function [X,Q_total,err_iter] = jointdiag_c_mz(X, options)


DEFAULT_MaxIter = 50;
DEFAULT_DisplayWarnings = 1;
DEFAULT_DisplayIter = 0;

threshold = 1e-8;

if nargin < 2
    DisplayIter = DEFAULT_DisplayIter;
    MaxIter = DEFAULT_MaxIter;
    DisplayWarnings = DEFAULT_DisplayWarnings;
else
    if isfield(options,'DisplayIter')
        DisplayIter = options.DisplayIter;
    else
        DisplayIter = DEFAULT_DisplayIter;
    end
    if isfield(options,'DisplayWarnings')
        DisplayWarnings = options.DisplayWarnings;
    else
        DisplayWarnings = DEFAULT_DisplayWarnings;
    end
    if isfield(options,'MaxIter')
        MaxIter = options.MaxIter;
    else
        MaxIter = DEFAULT_MaxIter;
    end
end

cellmode = 1;
if ~iscell(X)
    if ndims(X) ~= 3
        error('Input should be either a cell array of square matrices or a 3-dimensional array.');
    end
    cellmode = 0;
    X0 = X;
    X = cell(1,size(X0,3));
    for n = 1:length(X)
        X{n} = X0(:,:,n);
    end
end

R = length(X);


d = size(X{1},1);
for r = 1:R
    if any(size(X{r},1)~=d)
        error('This method requires all matrices to be square and of equal size.');
    end
end

if nargout>1
    Q_total = eye(d);
end


Converged = 0;


e = 0;
for r = 1:R
    for nx = 1:d-1
        for ny = nx+1:d
            e = e + abs(X{r}(nx,ny)).^2;
        end
    end
end

if DisplayIter
    fprintf('Sweep # 0: e = %g.\n',e);
end


if nargout > 2
    err_iter = zeros(1,MaxIter);
    err_iter(1) = e;

end

for k = 2:MaxIter
    smax = 0;
    for p = 1:d-1
        for q = p+1:d


            allbutpq = [1:p-1,p+1:q-1,q+1:d];
            
            
            sum1 = 0;sum2 = 0;sum3 = 0;
            for r = 1:R
                Cn = X{r}*X{r}' - X{r}'*X{r};
                sum1 = sum1 + Cn(p,q);
            end
                  
            alphak = angle ( sum1 ) - pi/2;
            
            for r = 1:R
     
                Gn = sum(abs(X{r}(p,allbutpq)).^2 + abs(X{r}(q,allbutpq)).^2 + (abs(X{r}(allbutpq,p)).^2 + abs(X{r}(allbutpq,q)).^2).');

                xin = exp(-1i*alphak)*X{r}(p,q) - exp(1i*alphak)*X{r}(q,p) ;
                dn = X{r}(p,p) - X{r}(q,q);
                sum2 = sum2 + abs(dn)^2 + abs(xin)^2;
                sum3 = sum3 + Gn;
            end


            yk = atanh(-abs(sum1) / (2*sum2 + sum3));
            
            Sk = eye(d);
            Sk(p,p) = cosh(yk);Sk(p,q) = sinh(yk)*(-1i)*exp(1i*alphak);
            Sk(q,p) = sinh(yk)*(1i)*exp(-1i*alphak);Sk(q,q) = cosh(yk);
            Ski = pinv(Sk);

            for r = 1:R
                X{r} = Ski*X{r}*Sk;

            end
            
            if nargout>1

                Q_total = Q_total * Sk;
            end
            
            smax = max(smax,abs(sinh(yk)));
            
            G = zeros(3,3);
            for r = 1:R
                hXr = [X{r}(p,p) - X{r}(q,q), X{r}(p,q) + X{r}(q,p), 1i*(X{r}(q,p) - X{r}(p,q))];
                G = G + real(hXr' * hXr);
            end
            [Q,L] = eig(G);
            [~,w] = sort(diag(L));
            xyz = Q(:,w(3));
            if xyz(1)<0 , xyz= -xyz; end ;
            c = sqrt(xyz(1)/2 + 1/2);
            s = 0.5*(xyz(2)-1i*xyz(3))/c;
            
            Uk = eye(d);
            Uk(p,p) = c;Uk(p,q) = -conj(s);
            Uk(q,p) = s;Uk(q,q) = c;
            
            for r = 1:R
                X{r} = Uk'*X{r}*Uk;
            end
            if nargout>1
                Q_total = Q_total * Uk;
            end            
            smax = max(smax,abs(s));
        end
    end
    olde = e;
    e = 0;
    for r = 1:R
        for nx = 1:d-1
            for ny = nx+1:d
                e = e + abs(X{r}(nx,ny)).^2;
            end
        end
    end
    if nargout > 2
        err_iter(k) = e;
    end
    if DisplayIter
        fprintf('Sweep # %d: e = %g.\n',k,e);
    end
    if smax<threshold
        Converged = 1;
        break
    end

end

if nargout > 2
    err_iter = err_iter(1:k);
end

if ~Converged
    if DisplayWarnings
        fprintf('Warning: Joint diagonalization did not converge (%g -> %g).\n',olde,e);
    end
end

if ~cellmode
    X0 = X;
    X = zeros(size(X0{1},1),size(X0{1},2),length(X0));
    for n = 1:size(X,3)
        X(:,:,n) = X0{n};
    end
end